
#include "Bullet.h"
#include <cmath>
#include <algorithm>
#include "Engine.h"
#include "Real2.h"

#define Sq(X) (X*X)
//#define normsq(X) Sq(X.x)+Sq(X.y)+Sq(X.z)
//#define norm(X) sqrt(normsq(X))
//#define zenith(X) acos(X.z/slength(X))
//#define azimuth(X) atan2(X.y, X.x)
//#define DOT(a, b) (a.x * b.x + a.y * b.y + a.z * b.z)
#define TWOPI 6.283185307179586476925287f

//
// Constructor
//

Bullet::Bullet()
{
}

//
// Update
//

void Bullet::Update()
{
	Real3 newPos = position + velocity * engine.lastFrameLength;

	// Drop from gravity
	newPos.y -= 0.5f * engine.gravity * Sq(engine.lastFrameLength);

	// Sets new position
	position = newPos;
}

//
// Test Collsion
//

/*float Bullet::BulletCollides( const BBox &box )
{
	float intersection = 0;

	Real3 vec = velocity * engine.lastFrameLength;
	Real3 C1 = box.Min() + Real3(          0.0f,           0.0f,           0.0f) - position;
	Real3 C2 = box.Min() + Real3(          0.0f,           0.0f, box.Extent().z) - position;
	Real3 C3 = box.Min() + Real3(          0.0f, box.Extent().y,           0.0f) - position;
	Real3 C4 = box.Min() + Real3(box.Extent().x,           0.0f,           0.0f) - position;
	Real3 C5 = box.Min() + Real3(box.Extent().x, box.Extent().y,           0.0f) - position;
	Real3 C6 = box.Min() + Real3(box.Extent().x,           0.0f, box.Extent().z) - position;
	Real3 C7 = box.Min() + Real3(          0.0f, box.Extent().y, box.Extent().z) - position;
	Real3 C8 = box.Min() + Real3(box.Extent().x, box.Extent().y, box.Extent().z) - position;

	Real2 sMin(	min(zenith(C1), min(zenith(C2), min(zenith(C3), min(zenith(C4),
							min(zenith(C5), min(zenith(C6), min(zenith(C7), zenith(C8)))))))),
							min(azimuth(C1), min(azimuth(C2), min(azimuth(C3), min(azimuth(C4),
							min(azimuth(C5), min(azimuth(C6), min(azimuth(C7), azimuth(C8)))))))));
	Real2 sMax(	max(zenith(C1), max(zenith(C2), max(zenith(C3), max(zenith(C4),
							max(zenith(C5), max(zenith(C6), max(zenith(C7), zenith(C8)))))))),
							max(azimuth(C1), max(azimuth(C2), max(azimuth(C3), max(azimuth(C4),
							max(azimuth(C5), max(azimuth(C6), max(azimuth(C7), azimuth(C8)))))))));
	float minslength(	min(slength(C1), min(slength(C2), min(slength(C3), min(slength(C4),
										min(slength(C5), min(slength(C6), min(slength(C7), slength(C8)))))))));

	if(zenith(vec) >= sMin.u && zenith(vec) <= sMax.u)
	if(azimuth(vec) >= sMin.v && azimuth(vec) <= sMax.v)
	if(slength(vec) >= minslength)
		intersection = minslength/slength(vec);

	return intersection;
}*/

float Bullet::BulletCollides2( const BBox &box )
{
	Real3 vec = velocity * engine.lastFrameLength;
	Real3 cor[] = { box.Min() + Real3(          0.0f,           0.0f,           0.0f) - position,
									box.Min() + Real3(          0.0f,           0.0f, box.Extent().z) - position,
									box.Min() + Real3(          0.0f, box.Extent().y,           0.0f) - position,
									box.Min() + Real3(box.Extent().x,           0.0f,           0.0f) - position,
									box.Min() + Real3(box.Extent().x, box.Extent().y,           0.0f) - position,
									box.Min() + Real3(box.Extent().x,           0.0f, box.Extent().z) - position,
									box.Min() + Real3(          0.0f, box.Extent().y, box.Extent().z) - position,
									box.Min() + Real3(box.Extent().x, box.Extent().y, box.Extent().z) - position };
	Real3 cen = box.Center() - position;
	Real3 intrsct = vec * (cen.LengthSq() / Dot(vec, cen));
	float angsum = 0;
	float intersection = 0;

	for(int i = 0; i < 7; ++i)
		for(int j = i+1; j < 8; ++j)
			if(cor[i].LengthSq() > cor[i].LengthSq())
			{ 
				Real3 temp = cor[i];
				cor[i] = cor[j];
				cor[j] = temp; 
			}
	cor[7] = cor[1];

	for(int i = 1; i < 8; ++i)
		cor[i] += cen * (1 - Dot(cen, cor[i]) / cen.LengthSq());

	for(int i=1; i<8; i++)
	{ 
		if(LengthSqOf(cor[i]-intrsct) < .01f)
			return intrsct.Length() / vec.Length();
		angsum += acos( Dot(cor[i]-intrsct, cor[i+1]-intrsct) / (LengthOf(cor[i]-intrsct) * LengthOf(cor[i+1]-intrsct)) ); 
	}

	if(angsum >= TWOPI-.01f)
		intersection = intrsct.Length() / vec.Length();

	return intersection;
}


/*bool PlaneXIntr(float x1 ,float y1 ,float z1,
								float x2 ,float y2 ,float z2,
								float px0,
								float py1,float py2,float pz1,float pz2)
{
	float t,l,m,n;
	l=x2-x1;
	m=y2-y1;
	n=z2-z1;
	if ( l==0.0f) return false;
	t=(px0-x1)/l;
	if ( t>=0.0f && t<=1.0f)
	{
		float py,pz;
		py=y1+t*m;
		pz=z1+t*n;
		if ( py>=py1 && py<=py2)
			if ( pz>=pz1 && pz<=pz2)
			{
				// px=x1+t*l;
				return true;
			}
	}
	return false;
}

bool PlaneYIntr(float x1 ,float y1 ,float z1,
								float x2 ,float y2 ,float z2,
								float py0,
								float px1,float px2,float pz1,float pz2)
{
	float t,l,m,n;
	l=x2-x1;
	m=y2-y1;
	n=z2-z1;
	if ( m==0.0f) return false;
	t=(py0-y1)/m;
	if ( t>=0.0f && t<=1.0f)
	{
		float px,pz;
		px=x1+t*l;
		pz=z1+t*n;
		if ( px>=px1 && px<=px2)
			if ( pz>=pz1 && pz<=pz2)
			{
				// py=y1+t*m;
				return true;
			}
	}
	return false;
}


bool PlaneZIntr(float x1 ,float y1 ,float z1,
								float x2 ,float y2 ,float z2,
								float pz0,
								float px1,float px2,float py1,float py2)
{
	float t,l,m,n;
	l=x2-x1;
	m=y2-y1;
	n=z2-z1;
	if ( n==0.0f) return false;
	t=(pz0-z1)/n;
	if ( t>=0.0f && t<=1.0f)
	{
		float px,py;
		px=x1+t*l;
		py=y1+t*m;
		if ( px>=px1 && px<=px2)
			if ( py>=py1 && py<=py2)
			{
				// pz=z1+t*n;
				return true;
			}
	}
	return false;
}

bool RayBoxTest(float x1 ,float y1 ,float z1,
								float x2 ,float y2 ,float z2,
								float ax ,float ay ,float az,
								float bx ,float by ,float bz)
{
	if ( PlaneXIntr(x1,y1,z1,x2,y2,z2,ax,ay,by,az,bz)==true ) return true;
	if ( PlaneXIntr(x1,y1,z1,x2,y2,z2,bx,ay,by,az,bz)==true ) return true;
	if ( PlaneYIntr(x1,y1,z1,x2,y2,z2,ay,ax,bx,az,bz)==true ) return true;
	if ( PlaneYIntr(x1,y1,z1,x2,y2,z2,by,ax,bx,az,bz)==true ) return true;
	if ( PlaneZIntr(x1,y1,z1,x2,y2,z2,az,ax,bx,ay,by)==true ) return true;
	if ( PlaneZIntr(x1,y1,z1,x2,y2,z2,bz,ax,bx,ay,by)==true ) return true;
	return false;
}*/